#include "cv.h"
#include "thread.h"
#include "control.h"
#include "options.h"
#include "ui.h"

#include <algorithm>
#include <opencv/highgui.h>
#include <opencv/cv.h>

static bool pic[CV_HEIGHT][CV_WIDTH];
static int s[CV_HEIGHT][CV_WIDTH];

char aippt_raw[100], aippt_dif[100];

int get_s(int s[CV_HEIGHT][CV_WIDTH], int x, int y)
{
    if (x >= CV_HEIGHT)
        x  = CV_HEIGHT - 1;
    if (y >= CV_WIDTH)
        y  = CV_WIDTH - 1;
    return x < 0 || y < 0 ? 0 : s[x][y];
}

int get_sum(int s[CV_HEIGHT][CV_WIDTH], int x1, int y1, int x2, int y2)
{
    return get_s(s, x2, y2) - get_s(s, x1 - 1, y2) - get_s(s, x2, y1 - 1)
            + get_s(s, x1 - 1, y1 - 1);
}

bool get_center(int x1, int y1, int x2, int y2, int& x, int& y)
{
    int tot = get_sum(s, x1 - 1, y1 - 1, x2, y2);
    
    for (int i = x1; i <= x2; ++i)
        if (get_sum(s, x1 - 1, y1 - 1, i, y2) * 3 >= tot)
        {
            x = i;
            break;
        }

    for (int i = y1; i <= y2; ++i)
        if (get_sum(s, x1 - 1, y1 - 1, x2, i) * 2 >= tot)
        {
            y = i;
            break;
        }
    int ctot = get_sum(s, x - 100, y - 200, x + 100, y + 200);
    if (ctot > DRAG_THRESHOLD_ACT && ctot * 4 > tot * 3)
        return true;
    else
    {
        x = y = -100;
        return false;
    }
}

double cv_check_hit(int& x, int& y)
{
    static int s[CV_HEIGHT], t[CV_WIDTH];
    memset(s, 0, sizeof s);
    memset(t, 0, sizeof t);

    int tot = 0;
    for (int i = 0; i < CV_HEIGHT; ++i)
        for (int j = 0; j < CV_WIDTH; ++j)
        {
            tot += pic[i][j];
            s[i] += pic[i][j];
            t[j] += pic[i][j];
        }

    int tmp = 0;
    for (int i = 0; i < CV_HEIGHT; ++i)
        if ((tmp += s[i]) * 2 > tot)
        {
            y = i;
            break;
        }
    tmp = 0;
    for (int j = 0; j < CV_WIDTH; ++j)
        if ((tmp += t[j]) * 2 > tot)
        {
            x = j;
            break;
        }
    static int ltot[HIT_FRAME], rtot[HIT_FRAME], lx[HIT_FRAME], rx[HIT_FRAME];
    static clock_t next_act = 0;
    double ret = 0.;
    if (clock() > next_act)
    {
        next_act += (clock_t)(0.1 * CLOCKS_PER_SEC);
        for (int i = HIT_FRAME - 1; i > 0; --i)
        {
            ltot[i] = ltot[i - 1];
            rtot[i] = rtot[i - 1];
            lx[i] = lx[i - 1] - HIT_RESIST_DIS;
            rx[i] = rx[i - 1] - HIT_RESIST_DIS;
        }
        ltot[0] = rtot[0] = lx[0] = rx[0] = 0;
        for (int j = 0; j < CV_WIDTH; ++j)
            if (j * 2 < CV_WIDTH)
            {
                ltot[0] += t[j];
                lx[0] += t[j] * (CV_WIDTH / 2 - j);
            }
            else
            {
                rtot[0] += t[j];
                rx[0] += t[j] * (j - CV_WIDTH / 2);
            }
        if (ltot[0])
            lx[0] /= ltot[0];
        else
            lx[0] = -1;
        if (rtot[0])
            rx[0] /= rtot[0];
        else
            rx[0] = -1;
        
        {
            bool ok = true;
            for (int i = 1; i < HIT_FRAME - 1; ++i)
                if (ltot[i] == 0 || rtot[i] == 0 ||
                    lx[i] + HIT_SPEED_LIMIT >= lx[i+1] ||
                    rx[i] + HIT_SPEED_LIMIT >= rx[i+1])
                    ok = false;
                else
                if (abs(ltot[i] - rtot[i]) < HIT_BALANCE_TOT &&
                    abs(lx[i] - rx[i]) < HIT_BALANCE_DIS)
                        ret += 0.2;
                else
                        ret += 0.1;

            if (ret > 0 && (ltot[0] + rtot[0] < HIT_NOISE))
                ret += 0.4;
            int ttx = 0;
            for (int i = (int)(CV_WIDTH * 0.4); i <= (int)(CV_WIDTH * 0.6); ++i)
                ttx += t[i];
            if (ret > 0.)
            {
                if (ttx < HIT_CENTER_PIXEL_RATE * tot)
                    ok = false;
                if (ttx >= HIT_CENTER_PIXEL_RATE * tot)
                    ret += 0.5;
                if (ttx >= HIT_CENTER_PIXEL_RATE * tot * 0.7)
                    ret += 0.2;
            }

            if ((ok && (ltot[0] + rtot[0] < HIT_NOISE)) || ret > 1.)
            {
                ret = 1.;
                lx[0] = -3000;
                lx[1] = -2000;
                lx[2] = -1000;
            }
        }
    }
        
    int tot_o = tot;
    tot *= NOISE_RATE_N;
    for (int i = std::max(0, y-CV_HEIGHT/2); 
                    i < std::min(CV_HEIGHT, y+CV_HEIGHT/5); ++i)
        for (int j = std::max(0, x-CV_WIDTH/4); 
                        j < std::min(CV_WIDTH, x+CV_WIDTH/4); ++j)
            tot -= NOISE_RATE_D * pic[i][j];
    if (tot > 0 || tot_o < THRESHOLD_NULL)
        x = y = -100;
    return ret;
}

static clock_t pt[MAX_VALID_POINT];
static double px[MAX_VALID_POINT], py[MAX_VALID_POINT];
static bool po[MAX_VALID_POINT];
static int now, valid_point;

void add_track_point(clock_t cur, double x, double y)
{
    now = (now + 1) % MAX_VALID_POINT;
    pt[now] = cur;
    px[now] = x;
    py[now] = y;
    
    po[now] = (x > -1 && x < CV_WIDTH + 1 && y > -1 && y < CV_HEIGHT + 1);
    
    if (valid_point < MAX_VALID_POINT - 1)
        ++valid_point;
}

bool get_point(clock_t u, double& x, double& y)
{
    int p = (now+MAX_VALID_POINT-1) % MAX_VALID_POINT;
    for (int i = 2; i < valid_point; ++i)
    {
        if (pt[p] < u) break;
        p = (p + MAX_VALID_POINT - 1) % MAX_VALID_POINT;
    }

    int q = (p + 1) % MAX_VALID_POINT;

    if (pt[p] >= u || pt[q] < u || !po[p] || !po[q])
    {
        x = y = 1E10;
        return false;
    }

    double r = double(u - pt[p]) / (pt[q] - pt[p]);

    x = px[p] + r * (px[q] - px[p]);
    y = py[p] + r * (py[q] - py[p]);
    return true;
}

bool valid_move_state(double totdx, double score)
{
    if (totdx > 0)
    {
        return (totdx > cv_move_x ||
               (totdx > cv_move_x * 0.8 && score > 2.0) ||
               (totdx > cv_move_x * 0.6 && score > 2.4));
    }

    totdx *= -1;
    {
        return (totdx > cv_move_x ||
               (totdx > cv_move_x * 0.8 && score > 2.0) ||
               (totdx > cv_move_x * 0.6 && score > 2.4));
    }

}

double cv_check_step(int x, int y, IplImage* f)
{
    static clock_t last_rec = -CLOCKS_PER_SEC;
    clock_t cur = clock();
    if (x > -1 && x < CV_WIDTH + 1 && y > -1 && y < CV_HEIGHT + 1)
        add_track_point(cur, x, y);
    else
        add_track_point(cur, 1E10, 1E10);
    
    double max_score = 2.5;
    double score = max_score, rs = 0, totdx = 0, tx, ty, ux = x, uy = y;
    bool uo = false, to;
    
    bool msign = false;

    for (int i = 1; i < MAX_SLICE; ++i)
    {
        if ((to = get_point(cur - i * TIME_SLICE, tx, ty)))
        {
            if ((tx - ux > 0) != (totdx > 0) || fabs(ty - uy) > SLICE_MAX_MOVE_Y ||
                fabs(tx - ux) > SLICE_MAX_MOVE_X ||
                fabs(tx - ux) < SLICE_MIN_MOVE_X)
                score -= 0.1;

            if (fabs(ty - uy) <= SLICE_MAX_MOVE_Y && fabs(tx - ux) <= SLICE_MAX_MOVE_X && 
                (tx > -1 && tx < CV_WIDTH + 1 && ty > -1 && ty < CV_HEIGHT + 1))
            {
                totdx += tx - ux;
            }
        } else
            score -= 0.1;

        
        if (uo && to)
            cvLine(f, cvPoint((int)(tx+.5), (int)(ty+.5)),cvPoint((int)(ux+.5), (int)(uy+.5)), CV_RGB((int)(200*(exp(-i*.03))),(int)(100*(exp(-i*.03))),0),2);
        
        if ((i >= MIN_MOVE_SLICE) && (i <= MAX_MOVE_SLICE) && 
            valid_move_state(totdx, score)) {
            if (score >= 1)
            {
                if (cur - last_rec > MOVE_DELAY)
                {
                    rs = 1.;
                    msign = totdx > 0;
                    last_rec = cur;
                }
                valid_point = 0;
            } else {
                if (score > rs)
                    rs = score, msign = totdx > 0;
            }
        }
        else if (score > 1)
        {
            if (fabs(totdx / cv_move_x) > rs)
            {
                rs = std::max(rs, fabs(totdx / cv_move_x));
                msign = totdx > 0;
            }
        }
        
        ux = tx, uy = ty, uo = to;
    }

    return rs * (msign?-1.:1.);
}

int sqr(int x)
{
    return x * x;
}

void cv_trace(int* dx, int* dy, int x, int y, bool new_round)
{
    static int xx, yy, tdx, tdy;
    *dx = *dy = 0;
    if (new_round || sqr(xx + tdx - x) + sqr(yy + tdy - y) > sqr(DRAG_RANGE))
    {
        tdx = 0;
        tdy = 0;
        xx = x;
        yy = y;
    } else
    {
        *dx = (int)((x-xx-tdx)*100./CV_WIDTH);
        *dy = (int)((y-yy-tdy)*100./CV_HEIGHT);
        tdx += *dx * CV_WIDTH / 100;
        tdy += *dy * CV_HEIGHT / 100;
        std::swap(*dx, *dy);
        *dy = -*dy;
    }
    return;
}

void cv_diff(IplImage* f1, IplImage* f2, IplImage* f, int diff_method)
{
    if (diff_method == DIFF_HARD)
    {
        for (int i = 0; i < CV_HEIGHT; ++i)
        {
            unsigned char* p1 = (unsigned char*)f1->imageData + i*f1->widthStep;
            unsigned char* p2 = (unsigned char*)f2->imageData + i*f2->widthStep;
            for (int j = 0; j < CV_WIDTH; ++j)
            {
                pic[i][j] = abs((int)(*p1) - *p2) + 
                    abs((int)(*(p1+1)) - *(p2+1)) +
                    abs((int)(*(p1+2)) - *(p2+2)) > cv_threshold;
                p1 += 3, p2 += 3;
            }
        }

        for (int i = 0; i < CV_HEIGHT; ++i)
            for (int j = 0; j < CV_WIDTH; ++j)
                s[i][j] = +get_s(s,i-1,j)+get_s(s,i,j-1)
                    -get_s(s,i-1,j-1)+pic[i][j];

        for (int i = 0; i < CV_HEIGHT; ++i)
            for (int j = 0; j < CV_WIDTH; ++j)
                pic[i][j] = 
                    get_sum(s, i - THRESHOLD_BOUND_X, j - THRESHOLD_BOUND_Y,
                            i + THRESHOLD_BOUND_X, j + THRESHOLD_BOUND_Y) > 
                    THRESHOLD_RATE * (THRESHOLD_BOUND_X*2+1) * 
                    (THRESHOLD_BOUND_Y*2+1);

        for (int i = 0; i < CV_HEIGHT; ++i)
        {
            unsigned char* p = (unsigned char*)f->imageData + f->widthStep * i;
            for (int j = 0; j < CV_WIDTH; ++j)
            {
                *p++ = pic[i][j] ? 0xFF : 0;
                *p++ = pic[i][j] ? 0xFF : 0;
                *p++ = pic[i][j] ? 0xFF : 0;
            }
        }
    
        for (int i = 0; i < CV_HEIGHT; ++i)
            for (int j = 0; j < CV_WIDTH; ++j)
            {
                s[i][j] = +get_s(s,i-1,j)+get_s(s,i,j-1)
                    -get_s(s,i-1,j-1)+pic[i][j];
            }
    } else
    {
        for (int i = 0; i < CV_HEIGHT; ++i)
        {
            unsigned char* p1 = (unsigned char*)f1->imageData + i*f1->widthStep;
            unsigned char* p2 = (unsigned char*)f2->imageData + i*f2->widthStep;
            unsigned char* p = (unsigned char*)f->imageData + i*f->widthStep;
            for (int j = 0; j < CV_WIDTH; ++j)
            {
                int t = abs((int)(*p1) - *p2) + 
                    abs((int)(*(p1+1)) - *(p2+1)) +
                    abs((int)(*(p1+2)) - *(p2+2));
                t = (int) ((t - 10) * (t - 10) * 0.005);
                t = std::min(std::max(t-DRAG_THERSHOLD_PIXEL,0),255);
                *p = *(p+1) = *(p+2) = (unsigned char)
                     t;
                p1 += 3, p2 += 3, p += 3;
                s[i][j] = +get_s(s,i-1,j)+get_s(s,i,j-1)
                    -get_s(s,i-1,j-1)+t;
            }
        }
    }
}

int cv_check_taiko()
{
    int st = 0;
    if (get_sum(s, 0, 0, CV_HEIGHT, CV_WIDTH / 2) * 16 > CV_WIDTH * CV_HEIGHT)
        st |= 1;

    if (get_sum(s, 0, CV_WIDTH/2+1, CV_HEIGHT, CV_WIDTH) * 16 > CV_WIDTH * CV_HEIGHT)
        st |= 2;
    return st;
}

void cv_create_windows(void)
{
    static int times = 0;
    snprintf(aippt_raw, 100, "(%d) Raw Input", times);
    snprintf(aippt_dif, 100, "(%d) Motion Detector", times);
    ++times;

    logf("%s %s\n", aippt_raw, aippt_dif);
    cvNamedWindow(aippt_raw, 1);
    cvNamedWindow(aippt_dif, 1);
    logf("I created two window\n");
}

void cv_destory_windows(void)
{
    //cvDestroyWindow(aippt_raw);
    //cvDestroyWindow(aippt_dif);
    cvDestroyAllWindows();
}

bool cv_check_destory_windows(void)
{
    HWND h1 = (HWND)cvGetWindowHandle(aippt_raw);
    HWND h2 = (HWND)cvGetWindowHandle(aippt_dif);
    if (!h1 || !h2 || !IsWindowVisible(h1) || !IsWindowVisible(h2)) {

        show_video = false;
        cv_destory_windows();
        return true;
    }
    return false;
}

void cv_main(void)
{
    CoInitialize(NULL);
    static CvCapture* cap = cvCaptureFromCAM(CV_CAP_ANY);
    static IplImage* f1 = cvCreateImage(cvSize(CV_WIDTH, CV_HEIGHT), 8, 3);
    static IplImage* f2 = cvCreateImage(cvSize(CV_WIDTH, CV_HEIGHT), 8, 3);
    static IplImage* f3 = cvCreateImage(cvSize(CV_WIDTH, CV_HEIGHT), 8, 3);
    static IplImage* f  = cvCreateImage(cvSize(CV_WIDTH, CV_HEIGHT), 8, 3);


    if (!cap)
    {
        MessageBox(NULL, L"找不到摄像头，请记得打开设备。", L"错误", MB_OK);
        exit(EXIT_FAILURE);
    }
    static result_t cv_swap;
    cv_swap.flag = 'o';
    static clock_t cl = clock();
    static bool last_show_video = false;
    
    for (int stamp = 0;; ++stamp)
    {
        cvCopy(f1, f2);

        set_result(&cv_swap);
        paint(NULL, true);
        change_result(&cv_swap);
        set_result(&cv_swap);
        paint(NULL, true);
        cv_swap.dx = cv_swap.dy = 0;

        cv_swap.fps = CLOCKS_PER_SEC / (double)(clock() - cl);
        
        cl = clock();

        cvResize(cvQueryFrame(cap), f1, 0);

        cvFlip(f1, f1, 1);
        cvCopy(f1, f3);

        
        if (!last_show_video && show_video) {
            last_show_video = true;
            cv_create_windows();
        }
        
        logf("%d\n",stamp);

        int xx, yy;
        if (cv_swap.flag == 'o' || cv_swap.flag == ',')
        {
                
            cv_diff(f1, f2, f, DIFF_HARD); // 差分
#ifdef EX_TAIKO
            {
                int st = cv_check_taiko();
                if (st == 3)
                {
                    cv_swap.flag_next = 'c';
                    printf(" Ren Da .....!\n");
                    cv_swap.status = 1.;
                    continue;
                }
                else if (st == 1)
                {
                    cv_swap.flag_next = 'a';
                    printf(" Dong.!\n");
                    cv_swap.status = 1.;
                    continue;
                }
                else if (st == 2)
                {
                    cv_swap.flag_next = 'b';
                    printf(" Ka.!\n");
                    cv_swap.status = 1.;
                    continue;
                }
            }
#endif


            double hit_s = cv_check_hit(xx, yy);
            double step_s = cv_check_step(xx, yy, f);
            if (hit_s > fabs(step_s)) {
                cv_swap.flag_next = cv_swap.flag == 'o' ? ',' : '.';
                cv_swap.status = hit_s;
            } else {
                cvRectangle(f, cvPoint(xx-40,yy-40),
                            cvPoint(xx+40,yy+40),CV_RGB(200,100,0),10);
                cvRectangle(f3, cvPoint(xx-40,yy-40),
                            cvPoint(xx+40,yy+40),CV_RGB(200,100,0),10);
                if (step_s >= 0)
                {
                    cv_swap.flag_next = cv_swap.flag == 'o' ? '<' : '-';
                    cv_swap.status = step_s;
                } else
                {
                    cv_swap.flag_next = cv_swap.flag == 'o' ? '>' : '+';
                    cv_swap.status = -step_s;
                }
            }
            
        } else
        if (cv_swap.flag == 'm')
        {
            static int last_stamp = -2;
            cv_diff(f1, f2, f, DIFF_SOFT); // 差分


            get_center(0,0,CV_HEIGHT, CV_WIDTH,xx,yy);
            get_center(xx-100,yy-100,xx+100, yy+100,xx,yy);

            cv_trace(&cv_swap.dx, &cv_swap.dy, xx, yy, stamp != last_stamp + 1);
            cvRectangle(f, cvPoint(yy-20,xx-20),
                        cvPoint(yy+20,xx+20),CV_RGB(200,100,0),10);
            cvRectangle(f3, cvPoint(yy-20,xx-20),
                        cvPoint(yy+20,xx+20),CV_RGB(200,100,0),10);
            last_stamp = stamp;
        }

        if (last_show_video && cv_check_destory_windows())
            last_show_video = false;
        cvWaitKey(CV_WAIT);

        if (last_show_video) {
            cvShowImage(aippt_dif, f);
            cvShowImage(aippt_raw, f3);
        }
    }

    cvReleaseImage(&f);
    cvReleaseImage(&f1);
    cvReleaseImage(&f2);
    cvReleaseCapture(&cap);
}
